﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace MassSpring
{
    public class ParticleSystem
    {
        private List<PhysicsNode> nodes = new List<PhysicsNode>();
        public List<PhysicsNode> Nodes
        {
            get { return nodes; }
        }

        private List<PhysicsNode> windTriangles = new List<PhysicsNode>();
        public List<PhysicsNode> WindTriangles
        {
            get { return windTriangles; }
        }

        private List<Spring> springs = new List<Spring>();
        public List<Spring> Springs
        {
            get { return springs; }
        }

        private Vector3 wind;
        public Vector3 Wind
        {
            get { return wind; }
            set { wind = value; }
        }

        private Vector3 gravity;
        public Vector3 Gravity
        {
            get { return gravity; }
            set { gravity = value; }
        }

        private float planeHeight;
        public float PlaneHeight
        {
            get { return planeHeight; }
        }

        public ParticleSystem(Vector3 gravity, Vector3 wind, float planeHeight)
        {
            this.gravity = gravity;
            this.wind = wind;
            this.planeHeight = planeHeight;
        }

        public void AddSpring(Spring spr)
        {
            Springs.Add(spr);
        }

        public void AddNode(PhysicsNode node)
        {
            Nodes.Add(node);
        }

        public void AddWindTriangle(PhysicsNode n1, PhysicsNode n2, PhysicsNode n3)
        {
            WindTriangles.Add(n1);
            WindTriangles.Add(n2);
            WindTriangles.Add(n3);
        }

        public void Integrate(float dt)
        {
            //Integrate using forward Euler
	
	        //Physics update here
	        //Apply simple gravity to each node
	        for (int i = 0 ; i < Nodes.Count; i++)
		        Nodes[i].AccumForce = (Gravity * Nodes[i].Mass);
	
	        //Apply wind force
	        for (int i = 0 ; i < WindTriangles.Count; i += 3)
	        {
		        Vector3 normal = Vector3.Cross((WindTriangles[i].Position - WindTriangles[i+1].Position),(
			        (WindTriangles[i].Position - WindTriangles[i+2].Position)));
		        normal.Normalize();
		        float dot = Vector3.Dot(normal,this.Wind);

		        Vector3 wind = dot * normal;

		        WindTriangles[i].AccumForce += wind * dt / 3;
		        WindTriangles[i+1].AccumForce += wind * dt / 3;
		        WindTriangles[i+2].AccumForce += wind * dt / 3;
	        }
	
	        //Apply springs
	        for (int i = 0 ; i < Springs.Count; i++)
		        Springs[i].Apply();

	        //Integrate the rope
	        for (int i = 0 ; i < Nodes.Count; i++)
	        {
		        if (!Nodes[i].Immovable && Nodes[i].Mass != 0.0f)
		        {
			        //Update vel.
			        Nodes[i].Velocity += Nodes[i].AccumForce * dt / Nodes[i].Mass;

			        //Damping
			        Nodes[i].Velocity -= ((1-Nodes[i].Damping)*dt) * Nodes[i].Velocity;

			        //Update pos.
			        Nodes[i].Position += Nodes[i].Velocity * dt;

		        }
		        Nodes[i].AccumForce = Vector3.Zero;
	        }

	        //TODO: Add proper ax + by + cz + d plane support
	        for (int i = 0 ; i < Nodes.Count; i++)
		        if (Nodes[i].Position.Y < PlaneHeight)
		        {
			        Nodes[i].Position = new Vector3(Nodes[i].Position.X, PlaneHeight,
                        Nodes[i].Position.Z);
			        Nodes[i].Velocity = Vector3.Zero;
		        }

	        //TODO: Use 4th order Runge-Kutta / Verlet for integration
        }

    }
}
